#include "mahony_filter.h"

/*************************************
完成时间：2023年09月02日 
功能介绍：实现mahony姿态角解算算法的模块封装
知乎账号：龙胆也
B站账号：华南小虎队
***************************************/

struct MAHONY_FILTER_t mahony_filter;
struct MAHONY_FILTER_t imu_filter;

/* 轻量级 atan2 近似实现，避免依赖 math 库，适合嵌入式实时场景 */
static inline float fast_atan2f(float y, float x)
{
    /* 参考近似：
       atan(z) ≈ z * (PI/4 + 0.273 * z^2) / (1 + z^2)
       通过象限判断组合为 atan2(y,x)
     */
    const float PI_2      = 1.57079632679490f;  /* π/2 */
    const float PI_4      = 0.78539816339745f;  /* π/4 */
    const float C         = 0.273f;

    if (x == 0.0f)
    {
        if (y > 0.0f)  return PI_2;
        if (y < 0.0f)  return -PI_2;
        return 0.0f; /* y == 0 */
    }

    float angle;
    if (x > 0.0f)
    {
        float z = y / x;
        float z2 = z * z;
        angle = z * (PI_4 + C * z2) / (1.0f + z2);
    }
    else /* x < 0 */
    {
        float z = y / x;
        float z2 = z * z;
        angle = z * (PI_4 + C * z2) / (1.0f + z2);
        if (y >= 0.0f)
            angle += PI;
        else
            angle -= PI;
    }
    return angle;
}
/*计算旋转矩阵*///大地坐标系 R 转换到机体坐标系 b 的坐标转换矩阵
void RotationMatrix_update(struct MAHONY_FILTER_t *mahony_filter)
{
    float q1q1 = mahony_filter->q1 * mahony_filter->q1;
    float q2q2 = mahony_filter->q2 * mahony_filter->q2;
    float q3q3 = mahony_filter->q3 * mahony_filter->q3;

    float q0q1 = mahony_filter->q0 * mahony_filter->q1;
    float q0q2 = mahony_filter->q0 * mahony_filter->q2;
    float q0q3 = mahony_filter->q0 * mahony_filter->q3;
    float q1q2 = mahony_filter->q1 * mahony_filter->q2;
    float q1q3 = mahony_filter->q1 * mahony_filter->q3;
    float q2q3 = mahony_filter->q2 * mahony_filter->q3;

    mahony_filter->rMat[0][0] = 1.0f - 2.0f * q2q2 - 2.0f * q3q3;
    mahony_filter->rMat[0][1] = 2.0f * (q1q2  -q0q3);
    mahony_filter->rMat[0][2] = 2.0f * (q1q3 +q0q2);

    mahony_filter->rMat[1][0] = 2.0f * (q1q2 +q0q3);
    mahony_filter->rMat[1][1] = 1.0f - 2.0f * q1q1 - 2.0f * q3q3;
    mahony_filter->rMat[1][2] = 2.0f * (q2q3  -q0q1);

    mahony_filter->rMat[2][0] = 2.0f * (q1q3  -q0q2);
    mahony_filter->rMat[2][1] = 2.0f * (q2q3  +q0q1);
    mahony_filter->rMat[2][2] = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;
}

// 将陀螺仪和加速度计的输入值赋予滤波器结构体
void mahony_input(struct MAHONY_FILTER_t *mahony_filter,Axis3f gyro,Axis3f acc)
{
    mahony_filter->gyro = gyro;
    mahony_filter->acc = acc;
}

// 使用Mahony算法更新滤波器状态
void mahony_update(struct MAHONY_FILTER_t *mahony_filter)
{
    float ex,ey,ez;
    
    /*角速度，度转弧度*/
//    mahony_filter->gyro.x *= DEG2RAD;        /* 度转弧度 */
//    mahony_filter->gyro.y *= DEG2RAD;
//    mahony_filter->gyro.z *= DEG2RAD;;
       
    /*单位化加速计测量值（使用 CMSIS-DSP sqrt 加速）*/
    float acc_ss = mahony_filter->acc.x * mahony_filter->acc.x
                 + mahony_filter->acc.y * mahony_filter->acc.y
                 + mahony_filter->acc.z * mahony_filter->acc.z;
    float acc_norm;
    if (arm_sqrt_f32(acc_ss, &acc_norm) != ARM_MATH_SUCCESS || acc_norm <= 0.0f)
    {
        acc_norm = 1.0f; /* 避免除零 */
    }
    mahony_filter->acc.x /= acc_norm;
    mahony_filter->acc.y /= acc_norm;
    mahony_filter->acc.z /= acc_norm;

    /*加速计读取的方向与重力加速计方向的差值，用向量叉乘计算*/
	//vx=mahony_filter->rMat[2][0]=2.0f * (q1q3 + -q0q2)
	//vy=mahony_filter->rMat[2][1]=2.0f * (q2q3 - -q0q1)
	//vz=mahony_filter->rMat[2][2]=1.0f - 2.0f * q1q1 - 2.0f * q2q2
    ex = (mahony_filter->acc.y * mahony_filter->rMat[2][2] - mahony_filter->acc.z * mahony_filter->rMat[2][1]);
    ey = (mahony_filter->acc.z * mahony_filter->rMat[2][0] - mahony_filter->acc.x * mahony_filter->rMat[2][2]);
    ez = (mahony_filter->acc.x * mahony_filter->rMat[2][1] - mahony_filter->acc.y * mahony_filter->rMat[2][0]);
    
    /*误差累计，与积分常数相乘*/
    mahony_filter->exInt += mahony_filter->Ki * ex * mahony_filter->dt ;  
    mahony_filter->eyInt += mahony_filter->Ki * ey * mahony_filter->dt ;
    mahony_filter->ezInt += mahony_filter->Ki * ez * mahony_filter->dt ;
    
    /*用叉积误差来做PI修正陀螺零偏，即抵消陀螺读数中的偏移量*/
    mahony_filter->gyro.x += mahony_filter->Kp * ex + mahony_filter->exInt;
    mahony_filter->gyro.y += mahony_filter->Kp * ey + mahony_filter->eyInt;
    mahony_filter->gyro.z += mahony_filter->Kp * ez + mahony_filter->ezInt;
    
    /* 一阶近似算法，四元数运动学方程的离散化形式和积分 */
    float q0Last = mahony_filter->q0;
    float q1Last = mahony_filter->q1;
    float q2Last = mahony_filter->q2;
    float q3Last = mahony_filter->q3;
    float halfT = mahony_filter->dt * 0.5f;
    mahony_filter->q0 += (-q1Last * mahony_filter->gyro.x - q2Last * mahony_filter->gyro.y - q3Last * mahony_filter->gyro.z) * halfT;
    mahony_filter->q1 += ( q0Last * mahony_filter->gyro.x + q2Last * mahony_filter->gyro.z - q3Last * mahony_filter->gyro.y) * halfT;
    mahony_filter->q2 += ( q0Last * mahony_filter->gyro.y - q1Last * mahony_filter->gyro.z + q3Last * mahony_filter->gyro.x) * halfT;
    mahony_filter->q3 += ( q0Last * mahony_filter->gyro.z + q1Last * mahony_filter->gyro.y - q2Last * mahony_filter->gyro.x) * halfT;
    
    /*单位化四元数（使用 CMSIS-DSP sqrt 加速）*/
    float q_ss = mahony_filter->q0 * mahony_filter->q0
               + mahony_filter->q1 * mahony_filter->q1
               + mahony_filter->q2 * mahony_filter->q2
               + mahony_filter->q3 * mahony_filter->q3;
    float q_norm;
    if (arm_sqrt_f32(q_ss, &q_norm) != ARM_MATH_SUCCESS || q_norm <= 0.0f)
    {
        q_norm = 1.0f; /* 避免除零 */
    }
    mahony_filter->q0 /= q_norm;
    mahony_filter->q1 /= q_norm;
    mahony_filter->q2 /= q_norm;
    mahony_filter->q3 /= q_norm;
    
    /*计算旋转矩阵*/
    mahony_filter->RotationMatrix_update(mahony_filter);
}

// 从旋转矩阵中提取姿态角
void mahony_output(struct MAHONY_FILTER_t *mahony_filter)
{
    /*计算roll pitch yaw 欧拉角（使用 CMSIS-DSP sqrt + 近似 atan2）*/
    /* pitch = -asin(rMat[2][0]) ≈ -atan2(rMat[2][0], sqrt(1 - rMat[2][0]^2)) */
    float t = mahony_filter->rMat[2][0];
    float one_minus_t2 = 1.0f - t * t;
    if (one_minus_t2 < 0.0f) one_minus_t2 = 0.0f; /* 数值保护 */
    float s;
    if (arm_sqrt_f32(one_minus_t2, &s) != ARM_MATH_SUCCESS)
    {
        s = 0.0f;
    }
    mahony_filter->pitch = -fast_atan2f(t, s);

    /* roll = atan2(rMat[2][1], rMat[2][2]) */
    mahony_filter->roll = fast_atan2f(mahony_filter->rMat[2][1], mahony_filter->rMat[2][2]);

    /* yaw = atan2(rMat[1][0], rMat[0][0]) */
    mahony_filter->yaw = fast_atan2f(mahony_filter->rMat[1][0], mahony_filter->rMat[0][0]);
}

// 初始化Mahony滤波器的参数和函数指针
void mahony_init(struct MAHONY_FILTER_t *mahony_filter,float Kp,float Ki,float dt)
{
    mahony_filter->Kp = Kp;
    mahony_filter->Ki = Ki;
    mahony_filter->dt = dt;
    mahony_filter->q0 = 1;
    mahony_filter->mahony_input = mahony_input;
    mahony_filter->mahony_update = mahony_update;
    mahony_filter->mahony_output = mahony_output;    
    mahony_filter->RotationMatrix_update = RotationMatrix_update;
}

